/*
 * motor.c
 *
 *  Created on: May 21, 2025
 *      Author: lsy18
 */

#include "motor.h"

#include "tim.h"

#include "../Communicate/Uart_slave/Uart_slave_REG/uart_slave_reg.h"

//#include "../Uart_slave/uart_data_deal.h"

void motor_init()
{
	HAL_TIM_PWM_Start(&htim_pwm, MOTOR_LEFT);
	HAL_TIM_PWM_Start(&htim_pwm, MOTOR_RIGHT);
	__HAL_TIM_SetCompare(&htim_pwm, MOTOR_LEFT, (htim_pwm.Init.Period+1)/100*motor_pwm_max);
	__HAL_TIM_SetCompare(&htim_pwm, MOTOR_RIGHT, (htim_pwm.Init.Period+1)/100*motor_pwm_max);
	HAL_Delay(3000);
	__HAL_TIM_SetCompare(&htim_pwm, MOTOR_LEFT, (htim_pwm.Init.Period+1)/100/2*(motor_pwm_max+motor_pwm_min));
	__HAL_TIM_SetCompare(&htim_pwm, MOTOR_RIGHT, (htim_pwm.Init.Period+1)/100/2*(motor_pwm_max+motor_pwm_min));
	HAL_Delay(3000);
}

void motor_control_single(uint16_t pwm_duty , MOTOR_SELECT motor_select)
{
	if(pwm_duty <= ((motor_pwm_min+1)*(htim_pwm.Init.Period+1)/100))
	{
		pwm_duty = (motor_pwm_min+1)*(htim_pwm.Init.Period+1)/100;
	}
	else if(pwm_duty >= ((motor_pwm_max-1)*(htim_pwm.Init.Period+1)/100))
	{
		pwm_duty = (motor_pwm_max-1)*(htim_pwm.Init.Period+1)/100;
	}
	uint8_t duty_buff[2];
	duty_buff[1] = (pwm_duty>>8)&0xFF;
	duty_buff[0] = pwm_duty&0xFF;
	if(motor_select == MOTOR_LEFT)
	{
		uart_slave_reg_write(duty_buff, PWM_LEFT_L_REG, 0X02);
	}
	else
	{
		uart_slave_reg_write(duty_buff, PWM_RIGHT_L_REG, 0X02);
	}
	__HAL_TIM_SetCompare(&htim_pwm, motor_select, pwm_duty);
}

void motor_control()
{
	uint8_t motor_buff[4];
	uart_slave_reg_read(motor_buff, PWM_LEFT_L_REG, 0X04);
	motor_control_single(motor_buff[0]|(motor_buff[1]<<8), MOTOR_LEFT);
	motor_control_single(motor_buff[2]|(motor_buff[3]<<8), MOTOR_RIGHT);
}
